
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.h
  * @author     baiyang
  * @date       2023-7-5
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "quadplane.h"
#include "define.h"

#include <assert.h>

#include <ahrs/ahrs_view.h>
#include <mb_arming/mb_arming.h>
#include <motors/mb_motors.h>
#include <rc_channel/rc_channel.h>
#include <common/location/location.h>
#include <mc_wpnav/mc_wpnav.h>
#include <srv_channel/srv_channel.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// Auto Pilot modes
// ----------------
enum ModeNumber {
    MN_MANUAL        = 0,
    MN_CIRCLE        = 1,
    MN_STABILIZE     = 2,
    MN_TRAINING      = 3,
    MN_ACRO          = 4,
    MN_FLY_BY_WIRE_A = 5,
    MN_FLY_BY_WIRE_B = 6,
    MN_CRUISE        = 7,
    MN_AUTOTUNE      = 8,
    MN_AUTO          = 10,
    MN_RTL           = 11,
    MN_LOITER        = 12,
    MN_TAKEOFF       = 13,
    MN_AVOID_ADSB    = 14,
    MN_GUIDED        = 15,
    MN_INITIALISING  = 16,
#if HAL_QUADPLANE_ENABLED
    MN_QSTABILIZE    = 17,
    MN_QHOVER        = 18,
    MN_QLOITER       = 19,
    MN_QLAND         = 20,
    MN_QRTL          = 21,
#if QAUTOTUNE_ENABLED
    MN_QAUTOTUNE     = 22,
#endif
    MN_QACRO         = 23,
#endif
    MN_THERMAL       = 24,
#if HAL_QUADPLANE_ENABLED
    MN_LOITER_ALT_QLAND = 25,
#endif
};

typedef struct ModeBase *mode_base_t;
typedef const struct ModeBase *mode_base_const_t;
typedef struct mode_ops *mode_ops_t;

struct ModeBase {
    mode_ops_t ops;

    ahrs_view *ahrs;
};

/** @ 
  * @brief  
  */
struct mode_ops {
    // run controllers specific to this mode
    void (*run)(mode_base_t mode);

    // returns a unique number specific to this mode
    enum ModeNumber (*mode_number)(mode_base_const_t mode);

    // returns full text name
    const char *(*name)(mode_base_const_t mode);

    // returns a string for this flightmode, exactly 4 bytes
    const char *(*name4)(mode_base_const_t mode);

    // convert user input to targets, implement high level control for this mode
    void (*update)(mode_base_t mode);

    // true for all q modes
    bool (*is_vtol_mode)(mode_base_const_t mode);
    bool (*is_vtol_man_throttle)(mode_base_const_t mode);
    bool (*is_vtol_man_mode)(mode_base_const_t mode);

    // guided or adsb mode
    bool (*is_guided_mode)(mode_base_const_t mode);

    // true if mode can have terrain following disabled by switch
    bool (*allows_terrain_disable)(mode_base_const_t mode);

    // true if automatic switch to thermal mode is supported.
    bool (*does_automatic_thermal_switch)(mode_base_const_t mode);

    // subclasses override this if they require navigation.
    void (*navigate)(mode_base_t mode);

    // this allows certain flight modes to mix RC input with throttle
    // depending on airspeed_nudge_cm
    bool (*allows_throttle_nudging)(mode_base_const_t mode);

    // true if the mode sets the vehicle destination, which controls
    // whether control input is ignored with STICK_MIXING=0
    bool (*does_auto_navigation)(mode_base_const_t mode);

    // true if the mode sets the vehicle destination, which controls
    // whether control input is ignored with STICK_MIXING=0
    bool (*does_auto_throttle)(mode_base_const_t mode);

    // true if the mode supports autotuning (via switch for modes other
    // that AUTOTUNE itself
    bool (*mode_allows_autotuning)(mode_base_const_t mode);

    // method for mode specific target altitude profiles
    void (*update_target_altitude)(mode_base_t mode);

    // handle a guided target request from GCS
    bool (*handle_guided_request)(mode_base_t mode, Location *target_loc);

    // true if is landing 
    bool (*is_landing)(mode_base_const_t mode);

    // true if is taking 
    bool (*is_taking_off)(mode_base_const_t mode);

    // subclasses override this to perform checks before entering the mode
    bool (*_enter)(mode_base_t mode);

    // subclasses override this to perform any required cleanup when exiting the mode
    void (*_exit)(mode_base_t mode);

    // mode specific pre-arm checks
    bool (*_pre_arm_checks)(mode_base_const_t mode, size_t buflen, char *buffer);
};

/// MODE
struct ModeInitializing {
    struct ModeBase mode;
};

struct ModeManual {
    struct ModeBase mode;
};

struct ModeAuto {
    struct ModeBase mode;
};

struct ModeFBWA {
    struct ModeBase mode;
};

struct ModeFBWB {
    struct ModeBase mode;
};

struct ModeTakeoff {
    struct ModeBase mode;
};

struct ModeStabilize {
    struct ModeBase mode;
};

struct ModeTraining {
    struct ModeBase mode;
};

struct ModeAcro {
    struct ModeBase mode;

    // ACRO controller state
    struct {
        bool locked_roll;
        bool locked_pitch;
        float locked_roll_err;
        int32_t locked_pitch_cd;
        Quat_t q;
        bool roll_active_last;
        bool pitch_active_last;
        bool yaw_active_last;
    } acro_state;
};

struct ModeAutoTune {
    struct ModeBase mode;
};

struct ModeGuided {
    struct ModeBase mode;
    float active_radius_m;
};

struct ModeCruise {
    struct ModeBase mode;
    bool locked_heading;
    int32_t locked_heading_cd;
    uint32_t lock_timer_ms;
};

struct ModeLoiter {
    struct ModeBase mode;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_ctor(mode_base_t mode, mode_ops_t ops);

void mode_exit(mode_base_t mode);
bool mode_enter(mode_base_t mode);

// returns true if the vehicle can be armed in this mode
bool mode_pre_arm_checks(mode_base_const_t mode, size_t buflen, char *buffer);

// Reset rate and steering controllers
void mode_reset_controllers(mode_base_t mode);

// Helper to output to both k_rudder and k_steering servo functions
void mode_output_rudder_and_steering(float val);

/// 
bool modeIsVtolManThrottle(mode_base_const_t mode);
void modeUpdateTargetAltitude(mode_base_t mode);
bool modePreArmChecks(mode_base_const_t mode, size_t buflen, char *buffer);
bool modeIsTakingOff(mode_base_const_t mode);
void modeRun(mode_base_t mode);

// run controllers specific to this mode
static inline void mode_run(mode_base_t mode) {
    if (mode->ops->run != NULL) {
        mode->ops->run(mode);
        return;
    }

    modeRun(mode);
}

// returns a unique number specific to this mode
static inline enum ModeNumber mode_number(mode_base_const_t mode) {
    assert(mode->ops->mode_number != NULL);
    return mode->ops->mode_number(mode);
}

// returns full text name
static inline const char *mode_name(mode_base_const_t mode) {
    assert(mode->ops->name != NULL);
    return mode->ops->name(mode);
}

// returns a string for this flightmode, exactly 4 bytes
static inline const char *mode_name4(mode_base_const_t mode) {
    assert(mode->ops->name4 != NULL);
    return mode->ops->name4(mode);
}

// convert user input to targets, implement high level control for this mode
static inline void mode_update(mode_base_t mode) {
    assert(mode->ops->update != NULL);
    mode->ops->update(mode);
}

// true for all q modes
static inline bool mode_is_vtol_mode(mode_base_const_t mode) {
    if (mode->ops->is_vtol_mode != NULL) {
        return mode->ops->is_vtol_mode(mode);
    }

    return false;
}

static inline bool mode_is_vtol_man_throttle(mode_base_const_t mode) {
    if (mode->ops->is_vtol_man_throttle != NULL) {
        return mode->ops->is_vtol_man_throttle(mode);
    }

    return modeIsVtolManThrottle(mode);
}

static inline bool mode_is_vtol_man_mode(mode_base_const_t mode) {
    if (mode->ops->is_vtol_man_mode != NULL) {
        return mode->ops->is_vtol_man_mode(mode);
    }

    return false;
}

// guided or adsb mode
static inline bool mode_is_guided_mode(mode_base_const_t mode) {
    if (mode->ops->is_guided_mode != NULL) {
        return mode->ops->is_guided_mode(mode);
    }

    return false;
}

// true if mode can have terrain following disabled by switch
static inline bool mode_allows_terrain_disable(mode_base_const_t mode) {
    if (mode->ops->allows_terrain_disable != NULL) {
        return mode->ops->allows_terrain_disable(mode);
    }

    return false;
}

// true if automatic switch to thermal mode is supported.
static inline bool mode_does_automatic_thermal_switch(mode_base_const_t mode) {
    if (mode->ops->does_automatic_thermal_switch != NULL) {
        return mode->ops->does_automatic_thermal_switch(mode);
    }

    return false;
}

// subclasses override this if they require navigation.
static inline void mode_navigate(mode_base_t mode) {
    if (mode->ops->navigate != NULL) {
        mode->ops->navigate(mode);
    }
}

// this allows certain flight modes to mix RC input with throttle
// depending on airspeed_nudge_cm
static inline bool mode_allows_throttle_nudging(mode_base_const_t mode) {
    if (mode->ops->allows_throttle_nudging != NULL) {
        return mode->ops->allows_throttle_nudging(mode);
    }

    return false;
}

// true if the mode sets the vehicle destination, which controls
// whether control input is ignored with STICK_MIXING=0
static inline bool mode_does_auto_navigation(mode_base_const_t mode) {
    if (mode->ops->does_auto_navigation != NULL) {
        return mode->ops->does_auto_navigation(mode);
    }

    return false;
}

// true if the mode sets the vehicle destination, which controls
// whether control input is ignored with STICK_MIXING=0
static inline bool mode_does_auto_throttle(mode_base_const_t mode) {
    if (mode->ops->does_auto_throttle != NULL) {
        return mode->ops->does_auto_throttle(mode);
    }

    return false;
}

// true if the mode supports autotuning (via switch for modes other
// that AUTOTUNE itself
static inline bool mode_allows_autotuning(mode_base_const_t mode) {
    if (mode->ops->mode_allows_autotuning != NULL) {
        return mode->ops->mode_allows_autotuning(mode);
    }

    return false;
}

// method for mode specific target altitude profiles
static inline void mode_update_target_altitude(mode_base_t mode) {
    if (mode->ops->update_target_altitude != NULL) {
        mode->ops->update_target_altitude(mode);
    } else {
        modeUpdateTargetAltitude(mode);
    }
}

// handle a guided target request from GCS
static inline bool mode_handle_guided_request(mode_base_t mode, Location *target_loc) {
    if (mode->ops->handle_guided_request != NULL) {
        return mode->ops->handle_guided_request(mode, target_loc);
    }

    return false;
}

// true if is landing 
static inline bool mode_is_landing(mode_base_const_t mode) {
    if (mode->ops->is_landing != NULL) {
        return mode->ops->is_landing(mode);
    }

    return false;
}

// true if is taking 
static inline bool mode_is_taking_off(mode_base_const_t mode) {
    if (mode->ops->is_taking_off != NULL) {
        return mode->ops->is_taking_off(mode);
    }

    return modeIsTakingOff(mode);
}

// subclasses override this to perform checks before entering the mode
static inline bool mode_enter2(mode_base_t mode) {
    if (mode->ops->_enter != NULL) {
        return mode->ops->_enter(mode);
    }

    return true;
}

// subclasses override this to perform any required cleanup when exiting the mode
static inline void mode_exit2(mode_base_t mode) {
    if (mode->ops->_exit != NULL) {
        mode->ops->_exit(mode);
    }
}

// mode specific pre-arm checks
static inline bool mode_pre_arm_checks2(mode_base_const_t mode, size_t buflen, char *buffer) {
    if (mode->ops->_pre_arm_checks != NULL) {
        return mode->ops->_pre_arm_checks(mode, buflen, buffer);
    }

    return modePreArmChecks(mode, buflen, buffer);
}


/// mode initializing
void minit_ctor(struct ModeInitializing *mode_initializing);

/// mode manual
void mmanual_ctor(struct ModeManual *mode_manual);

/// mode fbwa
void mfbwa_ctor(struct ModeFBWA *mode_fbwa);

/// mode stabilize
void mstabilize_ctor(struct ModeStabilize *mode_stabilize);

/// mode acro
void macro_ctor(struct ModeAcro *mode_acro);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



